import CONSTANT from '../../constant';
import JointLink from "./joint-link";
import PgsJointConstraintSolver from "../solver/pgs-joint-constraint-solver";
import DirectJointConstraintSolver from "../solver/direct/direct-joint-constraint-solver";
import JointImpulse from "./joint-impulse";
import Vec3 from "../../common/vec3";
import Mat3 from "../../common/mat3";
import Method from "../../common/method";

export default class Joint {
    constructor(config,type) {
        this._link1 = new JointLink(this);
        this._link2 = new JointLink(this);
        this._positionCorrectionAlgorithm = CONSTANT.SETTING_DEFAULT_JOINT_POSITION_CORRECTION_ALGORITHM;
        this._type = type;
        this._world = null;
        this._b1 = config.rigidBody1;
        this._b2 = config.rigidBody2;
        this._allowCollision = config.allowCollision;
        this._breakForce = config.breakForce;
        this._breakTorque = config.breakTorque;
        switch(config.solverType) {
            case CONSTANT.CONSTRAINT_SOLVER_TYPE_ITERATIVE:
                this._solver = new PgsJointConstraintSolver(this);
                break;
            case CONSTANT.CONSTRAINT_SOLVER_TYPE_DIRECT:
                this._solver = new DirectJointConstraintSolver(this);
                break;
        }
        this.localAnchor1=new Float64Array(3);
        Method.copyElements(config.localAnchor1.elements,this.localAnchor1);
        this.localAnchor2=new Float64Array(3);
        Method.copyElements(config.localAnchor2.elements,this.localAnchor2);
        this.relativeAnchor1 = new Float64Array(3);
        this.relativeAnchor2 = new Float64Array(3);
        this.anchor1=new Float64Array(3);
        this.anchor2 = new Float64Array(3);
        this.localBasis1 = new Float64Array(9);
        this.basis1 = new Float64Array(9);
        this.localBasis2 = new Float64Array(9);
        this.basis2 = new Float64Array(9);
        this.appliedForce=new Float64Array(3);
        this.appliedTorque=new Float64Array(3);
        this._prev=null;
        this._next=null;
        let _g1 = CONSTANT.SETTING_MAX_JACOBIAN_ROWS;
        this._impulses = new Array(_g1);
        let _g = 0;
        while(_g < _g1) this._impulses[_g++] = new JointImpulse();
        this._tv=new Float64Array(4);
        this._tm=new Float64Array(9);
        this._tva=new Float64Array(4);
        this._tvb=new Float64Array(4);
        this._tvc=new Float64Array(4);
        this._tvd=new Float64Array(4);
    }
    _setAxis(els,start=0,x=1,y=0,z=0){
        let l=els[start] * els[start] + els[start+1] * els[start+1] + els[start+2] * els[start+2];
        if( l=== 0) {
            Method.setXYZ(els,x,y,z,start);
        } else {
            Method.scaleArray(els,l>0?1/Math.sqrt(l):l,els,start,start,3);
        }
    }
    buildLocalBasesFromX() {
        let b1=this.localBasis1,b2=this.localBasis2;
        let tv=this._tv,tm=this._tm;
        this._setAxis(b1,0,1,0,0);
        this._setAxis(b2,0,1,0,0);
        Method.setRotFromTwoVec3(b1[0],b1[1],b1[2],b2[0],b2[1],b2[2],tv,tm);
        Method.vecToQuat(b1[0],b1[1],b1[2],tv);
        Method.setXYZ(b1,tv[0],tv[1],tv[2],3);
        Method.setXYZ(b1,b1[1] * b1[5] - b1[2] * b1[4],b1[2] * b1[3] - b1[0] * b1[5],b1[0] * b1[4] - b1[1] * b1[3],6);
        Method.multiplyBasis(tm,b1,b2);
    }
    _setAxisYZ(els){
        let tv=this._tv;
        let l=els[6] * els[6] + els[7] * els[7] + els[8] * els[8];
        if( l=== 0) {
            Method.vecToQuat(els[0],els[1],els[2],tv);
            Method.setXYZ(els,tv[0],tv[1],tv[2],3);
            Method.crossVectors(els[0],els[1],els[2],els[3],els[4],els[5],els,6);
        } else {
            Method.scaleArray(els,l>0?1 / Math.sqrt(l):l,els,6,6,3);
            Method.crossVectors(els[6],els[7],els[8],els[0],els[1],els[2],els,3);
        }
    }
    buildLocalBasesFromXY() {
        let bs1=this.localBasis1,bs2=this.localBasis2;
        this._setAxis(bs1,0,1,0,0);
        this._setAxis(bs2,0,1,0,0);
        Method.crossVectors(bs1[0],bs1[1],bs1[2],bs1[3],bs1[4],bs1[5],bs1,6);
        Method.crossVectors(bs2[0],bs2[1],bs2[2],bs2[3],bs2[4],bs2[5],bs2,6);
        this._setAxisYZ(bs1);
        this._setAxisYZ(bs2);
    }
    buildLocalBasesFromX1Z2() {
        let bs1=this.localBasis1,bs2=this.localBasis2;
        this._setAxis(bs1,0,1,0,0);
        this._setAxis(bs2,6,0,0,1);
        let tf1 = this._b1._transform.elements;
        let tf2 = this._b2._transform.elements;
        let wX1=this._tv,wZ2=this._tva,wY=this._tvb,wZ1=this._tvc,wX2=this._tvd;
        Method.transformVec3(tf1,bs1,1,wX1,0);
        Method.transformVec3(tf2,bs2,1,wZ2,6);
        Method.crossVectors(wZ2[0],wZ2[1],wZ2[2],wX1[0],wX1[1],wX1[2],wY,0);
        if(wY[0] * wY[0] + wY[1] * wY[1] + wY[2] * wY[2] === 0) {
            Method.vecToQuat(wX1[0],wX1[1],wX1[2],wY);
        }
        Method.crossVectors(wX1[0],wX1[1],wX1[2],wY[0],wY[1],wY[2],wZ1,0);
        Method.crossVectors(wY[0],wY[1],wY[2],wZ2[0],wZ2[1],wZ2[2],wX2,0);
        Method.inverseTransformVec3(tf1,wX1,1,bs1,0,0);
        Method.inverseTransformVec3(tf1,wY,1,bs1,0,3);
        Method.inverseTransformVec3(tf1,wZ1,1,bs1,0,6);
        Method.inverseTransformVec3(tf2,wX2,1,bs2,0,0);
        Method.inverseTransformVec3(tf2,wY,1,bs2,0,3);
        Method.inverseTransformVec3(tf2,wZ2,1,bs2,0,6);
    }
    buildLocalBasesFromXY1X2() {
        let bs1=this.localBasis1,bs2=this.localBasis2;
        let tv=this._tv,tm=this._tm;
        this._setAxis(bs1,0,1,0,0);
        Method.crossVectors(bs1[0],bs1[1],bs1[2],bs1[3],bs1[4],bs1[5],bs1,6);
        this._setAxisYZ(bs1);
        Method.setRotFromTwoVec3(bs1[0],bs1[1],bs1[2],bs2[0],bs2[1],bs2[2],tv,tm);
        Method.multiplyBasis(tm,bs1,bs2);
    }
    _setMotor(isPositionPart,sd,row,timeStep,lm,slopDefault,zetaDefault,value,out=new Float64Array(3)){
        let cfmFactor,erp;
        let slop = slopDefault;
        if(isPositionPart) {
            cfmFactor = 0;
            erp = 1;
        } else {
            if(sd.frequency > 0) {
                slop = 0;
                let omega = 6.28318530717958 * sd.frequency;
                let zeta = sd.dampingRatio;
                if(zeta < zetaDefault) {
                    zeta = zetaDefault;
                }
                let h = timeStep.dt;
                let c = 2 * zeta * omega;
                let k = omega * omega;
                if(sd.useSymplecticEuler) {
                    cfmFactor = 1 / (h * c);
                    erp = k / c;
                } else {
                    cfmFactor = 1 / (h * (h * k + c));
                    erp = k / (h * k + c);
                }
            } else {
                cfmFactor = 0;
                erp = this.getErp(timeStep,false);
            }
            if(value > 0) {
                row.motorSpeed = lm.motorSpeed;
                row.motorMaxImpulse = value * timeStep.dt;
            } else {
                row.motorSpeed = 0;
                row.motorMaxImpulse = 0;
            }
        }
        out[0]=cfmFactor;
        out[1]=erp;
        out[2]=slop;
    }
    _setRowImpulseInfo(row,lm,diff,slop,cfmFactor,erp,mass,lower,upper){
        let minImp, maxImp, error;
        if(lower > upper) {
            minImp = maxImp = error = 0;
        } else if(lower === upper) {
            minImp = -1e65536;
            maxImp = 1e65536;
            error = diff - lower;
        } else if(diff < lower) {
            minImp = -1e65536;
            maxImp = 0;
            error = diff - lower + slop;
            if(error > 0) {
                error = 0;
            }
        } else if(diff > upper) {
            minImp = 0;
            maxImp = 1e65536;
            error = diff - upper - slop;
            if(error < 0) {
                error = 0;
            }
        } else {
            minImp = maxImp = error = 0;
        }
        row.minImpulse = minImp;
        row.maxImpulse = maxImp;
        row.cfm = cfmFactor * (mass === 0 ? 0 : 1 / mass);
        row.rhs = error * erp;
    }
    setSolverInfoRowLinear(row,diff,lm,mass,sd,timeStep,isPositionPart) {
        const SLS=CONSTANT.SETTING_LINEAR_SLOP;
        const SDR=CONSTANT.SETTING_MIN_SPRING_DAMPER_DAMPING_RATIO;
        let tv=this._tv;
        this._setMotor(isPositionPart,sd,row,timeStep,lm,SLS,SDR,lm.motorForce,tv);
        let cfmFactor=tv[0];
        let erp=tv[1];
        let slop = tv[2];
        let lower = lm.lowerLimit;
        let upper = lm.upperLimit;
        this._setRowImpulseInfo(row,lm,diff,slop,cfmFactor,erp,mass,lower,upper);
    }
    setSolverInfoRowAngular(row,diff,lm,mass,sd,timeStep,isPositionPart) {
        const SAS=CONSTANT.SETTING_ANGULAR_SLOP;
        const SDR=CONSTANT.SETTING_MIN_SPRING_DAMPER_DAMPING_RATIO;
        let tv=this._tv;
        this._setMotor(isPositionPart,sd,row,timeStep,lm,SAS,SDR,lm.motorTorque,tv);
        let cfmFactor=tv[0];
        let erp=tv[1];
        let slop = tv[2];
        let lower = lm.lowerLimit;
        let upper = lm.upperLimit;
        let mid = (lower + upper) * 0.5;
        diff -= mid;
        diff = ((diff + 3.14159265358979) % 6.28318530717958 + 6.28318530717958) % 6.28318530717958 - 3.14159265358979;
        diff += mid;
        this._setRowImpulseInfo(row,lm,diff,slop,cfmFactor,erp,mass,lower,upper);
    }
    getErp(timeStep,isPositionPart) {
        const PCB=CONSTANT.POSITION_CORRECTION_ALGORITHM_BAUMGARTE;
        const SVB=CONSTANT.SETTING_VELOCITY_BAUMGARTE;
        if(isPositionPart) {
            return 1;
        } else if(this._positionCorrectionAlgorithm === PCB) {
            return timeStep.invDt * SVB;
        } else {
            return 0;
        }
    }
    computeEffectiveInertiaMoment(axisX,axisY,axisZ) {
        let ii1=this._b1.invInertia, ii2=this._b2.invInertia;
        let ra1=this.relativeAnchor1,ra2=this.relativeAnchor2;
        let ia1=this._tv,ia2=this._tva;
        Method.rotateVecTo(axisX,axisY,axisZ,ii1,ia1);
        Method.rotateVecTo(axisX,axisY,axisZ,ii2,ia2);
        let invI1=Method.inverseInertia(axisX,axisY,axisZ,ia1,ra1,this._b1._invMass,this._b1._mass);
        let invI2=Method.inverseInertia(axisX,axisY,axisZ,ia2,ra2,this._b2._invMass,this._b2._mass);
        return (invI1 + invI2 === 0)?0:1 / (invI1 + invI2);
    }
    computeEffectiveInertiaMoment2(axis1X,axis1Y,axis1Z,axis2X,axis2Y,axis2Z) {
        let ii1=this._b1.invInertia, ii2=this._b2.invInertia;
        let ra1=this.relativeAnchor1,ra2=this.relativeAnchor2;
        let ia1=this._tv,ia2=this._tva;
        Method.rotateVecTo(axis1X,axis1Y,axis1Z,ii1,ia1);
        Method.rotateVecTo(axis2X,axis2Y,axis2Z,ii2,ia2);
        let invI1=Method.inverseInertia(axis1X,axis1Y,axis1Z,ia1,ra1,this._b1._invMass,this._b1._mass);
        let invI2=Method.inverseInertia(axis2X,axis2Y,axis2Z,ia2,ra2,this._b2._invMass,this._b2._mass);
        return (invI1 + invI2 === 0)?0:1 / (invI1 + invI2);
    }
    _syncAnchors() {
        let bs1=this.localBasis1,la1=this.localAnchor1,ra1=this.relativeAnchor1,a1=this.anchor1,b1=this.basis1;
        let bs2=this.localBasis2,la2=this.localAnchor2,ra2=this.relativeAnchor2,a2=this.anchor2,b2=this.basis2;
        let tf1 = this._b1._transform.elements, tf2 = this._b2._transform.elements;
        Method.transformVec3(tf1,la1,1,ra1);
        Method.transformVec3(tf2,la2,1,ra2);
        Method.transformVec3(tf1,ra1,2,a1);
        Method.transformVec3(tf2,ra2,2,a2);
        Method.transformM3(tf1,bs1,b1);
        Method.transformM3(tf2,bs2,b2);
    }
    _getVelocitySolverInfo(timeStep,info) {
        info.b1 = this._b1;
        info.b2 = this._b2;
        info.numRows = 0;
    }
    _getPositionSolverInfo(info) {
        info.b1 = this._b1;
        info.b2 = this._b2;
        info.numRows = 0;
    }
    _checkDestruction() {
        let at=this.appliedTorque,af=this.appliedForce,bf=this._breakForce,bt=this._breakTorque;
        let torqueSq = at[0] * at[0] + at[1] * at[1] + at[2] * at[2];
        if(bf > 0 && af[0] * af[0] + af[1] * af[1] + af[2] * af[2] > bf * bf) {
            this._world.removeJoint(this);
            return;
        }
        if(bt > 0 && torqueSq > bt * bt) {
            this._world.removeJoint(this);
        }
    }
    _resetRow(row,impulse){
        Method.fillValue(row.jacobian.elements,0,11,0);
        row.rhs = 0;
        row.cfm = 0;
        row.minImpulse = 0;
        row.maxImpulse = 0;
        row.motorSpeed = 0;
        row.motorMaxImpulse = 0;
        row.impulse = null;
        row.impulse = impulse;
    }
    getRigidBody1() {
        return this._b1;
    }
    getRigidBody2() {
        return this._b2;
    }
    getType() {
        return this._type;
    }
    getAnchor1() {
        return new Vec3(this.anchor1[0],this.anchor1[1],this.anchor1[2]);
    }
    getAnchor2() {
        return new Vec3(this.anchor2[0],this.anchor2[1],this.anchor2[2]);
    }
    getAnchor1To(anchor) {
        Method.setXYZ(anchor,this.anchor1[0],this.anchor1[1],this.anchor1[2]);
    }
    getAnchor2To(anchor) {
        Method.setXYZ(anchor,this.anchor2[0],this.anchor2[1],this.anchor2[2]);
    }
    getLocalAnchor1() {
        return new Vec3(this.localAnchor1[0],this.localAnchor1[1],this.localAnchor1[2]);
    }
    getLocalAnchor2() {
        return new Vec3(this.localAnchor2[0],this.localAnchor2[1],this.localAnchor2[2]);
    }
    getLocalAnchor1To(localAnchor) {
        Method.setXYZ(localAnchor,this.localAnchor1[0],this.localAnchor1[1],this.localAnchor1[2]);
    }
    getLocalAnchor2To(localAnchor) {
        Method.setXYZ(localAnchor,this.localAnchor2[0],this.localAnchor2[1],this.localAnchor2[2]);
    }
    getBasis1() {
        let m = new Mat3();
        Method.copyElements(this.basis1,m.elements);
        return m;
    }
    getBasis2() {
        let m = new Mat3();
        Method.copyElements(this.basis2,m.elements);
        return m;
    }
    getBasis1To(basis) {
        let b=this.basis1;
        Method.setM3X3(basis,b[0],b[1],b[2],b[3],b[4],b[5],b[6],b[7],b[8]);
    }
    getBasis2To(basis) {
        let b=this.basis2;
        Method.setM3X3(basis,b[0],b[1],b[2],b[3],b[4],b[5],b[6],b[7],b[8]);
    }
    getAllowCollision() {
        return this._allowCollision;
    }
    setAllowCollision(allowCollision) {
        this._allowCollision = allowCollision;
    }
    getBreakForce() {
        return this._breakForce;
    }
    setBreakForce(breakForce) {
        this._breakForce = breakForce;
    }
    getBreakTorque() {
        return this._breakTorque;
    }
    setBreakTorque(breakTorque) {
        this._breakTorque = breakTorque;
    }
    getPositionCorrectionAlgorithm() {
        return this._positionCorrectionAlgorithm;
    }
    setPositionCorrectionAlgorithm(positionCorrectionAlgorithm) {
        if(positionCorrectionAlgorithm!==CONSTANT.POSITION_CORRECTION_ALGORITHM_BAUMGARTE&&
        positionCorrectionAlgorithm!==CONSTANT.POSITION_CORRECTION_ALGORITHM_NGS&&
        positionCorrectionAlgorithm!==CONSTANT.POSITION_CORRECTION_ALGORITHM_SPLIT_IMPULSE){
            throw new Error("invalid position correction algorithm id: " + positionCorrectionAlgorithm);
        }
        this._positionCorrectionAlgorithm = positionCorrectionAlgorithm;
    }
    getAppliedForce() {
        return new Vec3(this.appliedForce[0],this.appliedForce[1],this.appliedForce[2]);
    }
    getAppliedForceTo(appliedForce) {
        Method.setXYZ(appliedForce,this.appliedForce[0],this.appliedForce[1],this.appliedForce[2]);
    }
    getAppliedTorque() {
        return new Vec3(this.appliedTorque[0],this.appliedTorque[1],this.appliedTorque[2]);
    }
    getAppliedTorqueTo(appliedTorque) {
        Method.setXYZ(appliedTorque,this.appliedTorque[0],this.appliedTorque[1],this.appliedTorque[2]);
    }
    getPrev() {
        return this._prev;
    }
    getNext() {
        return this._next;
    }
}